#include <vector>
#include <algorithm>
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/SetMavFrame.h>

#include <geometry_msgs/TwistStamped.h>
 
#include <geometry_msgs/QuaternionStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <sensor_msgs/NavSatFix.h>
#include <std_msgs/Float32.h>
#include <std_msgs/UInt8.h>
#include <std_msgs/Bool.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <tf/tf.h>
#include <sensor_msgs/LaserScan.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/Joy.h>
#include <sensor_msgs/Imu.h> 
#include <std_msgs/Float32MultiArray.h>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
// #include <sensor_msgs/LaserScan.h>  qaq
#include <mavros_msgs/RCIn.h>
#include <mavros_msgs/OverrideRCIn.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <mavros_msgs/Altitude.h>
#include <mavros_msgs/ActuatorControl.h>
#include <mavros_msgs/WaypointList.h>


#include "quadrotor_msgs/PositionCommand.h"
#include <trajectory_msgs/MultiDOFJointTrajectory.h>
#include <unionsys_core.hpp>


RegisterMSN *unionsys_core = new RegisterMSN();

enum FSM_STATE { INIT, TAKEOFF, CRUISE, PLANNER, GO_PIPELINE, PIPELINE, GO_CIRCLE, CIRCLE, GO_TRACKING, TRACKING, GO_STAIRS, STAIRS, GO_EXPLORATION, EXPLORATION, RETURN};
std::string state_str[15] = { "INIT",
                        "TAKEOFF",
                        "CRUISE",
                        "PLANNER", 
                        "GO_PIPELINE",
                        "PIPELINE",
                        "GO_CIRCLE",
                        "CIRCLE",
                        "GO_TRACKING",
                        "TRACKING",
                        "GO_STAIRS",
                        "STAIRS",
                        "GO_EXPLORATION",
                        "EXPLORATION",
                        "RETURN"};
FSM_STATE exec_state;

ros::Time last_offboard_request;
ros::Time last_pub_goal_time;
ros::Time last_pub_tracking_time;
ros::Time last_path_send;

mavros_msgs::CommandBool arm_cmd;
mavros_msgs::SetMode offb_set_mode;
mavros_msgs::SetMavFrame setmavframe;

//控制pid
float P_pos = 1.0;
float I_pos = 0.0;
float D_pos = 0.2;

const float P_yaw = 1.00;
const float I_yaw = 0.00;
const float D_yaw = 0.00;
const float P_z = 1.00;

double last_calc_time = 0;

const float YAW_I_LIMIT = 2.0;
const float YAW_RATE_LIMIT = 15.0;

#define POS_I_DEAD_ZONE 0.5 
#define YAW_DEAD_ZONE 0
#define HEIGHT_DEAD_ZONE 0.05

//rc channels
int channel1_value;
int channel2_value;
int channel3_value;
int channel4_value;
int channel5_value;
int channel6_value;
int channel7_value;
int channel8_value;

const float deg2rad = 3.1415926535798/180.0;
const float rad2deg = 180.0/3.1415926535798;

//launch file variables
double mission_number;
bool use_sitl;
double max_vel_beishu;

double planner_start_point_x;
double planner_start_point_y;
double planner_start_point_z;

double planner_target_point_x;
double planner_target_point_y;
double planner_target_point_z;



int vel_command_cnt = 0;
int print_state_cnt = 0;
int waypoint_exec_num = 0;
int pipline_waypoint_num = 0;

int test_cnt = 0;

int planner_cnt = 0;


//ros node define
mavros_msgs::State current_state;

ros::Subscriber local_odom_sub;
ros::Subscriber rcin_sub;
ros::Subscriber state_sub;
ros::Subscriber planWaypointsub;
ros::Subscriber velocitysub;
ros::Subscriber altitude_sub; 

ros::Publisher  local_vel_pub, simple_goal_pub_, control_plot;
ros::Publisher  max_vel_pub;

ros::ServiceClient arming_client;
ros::ServiceClient set_mode_client;
ros::ServiceClient set_vechile_ned;

ros::Timer px4_control_timer;

geometry_msgs::Point current_point, body_vel;
geometry_msgs::Point planned_point;
geometry_msgs::Point planned_velocity;
geometry_msgs::Point current_vel;
geometry_msgs::Quaternion current_angle;
geometry_msgs::Vector3 curr_plane_angle;

geometry_msgs::Point error_pos_last, error_pos_integrated, attitude_expect, velocity_expected;

float error_yaw_last = 0.0, error_yaw_integrated = 0.0;


bool planned_update_flag = false;
bool pub_planner_goal_once = true;
bool pub_planner_waypoints_once = true;

float planned_yaw_angle;
float planned_yaw_dot;

Eigen::Quaterniond current_angle_Q;

template <typename T>
T limit(T a, T max)
{
  if(a > max)
    a = max;
  if(a < -max)
    a = -max;
  return a;
}

bool local_odom_flag = false;
double current_point_z = 0.0;

void position_pid_control(geometry_msgs::Point current_set_point,geometry_msgs::Point current_local_point,float velocity_limit,float target_yaw, float dead_zone, bool ifplan);
void vel_command_send(geometry_msgs::Point vel, double yaw_rate);
void changeFSMExecState(FSM_STATE new_state, std::string pos_call);
void printFSMExecState();


void set_cruise_state();
void waypoints_no_planner(mavros_msgs::WaypointList waypoints_plan, double yaw, double yuzhi);
void waypoints_planner(mavros_msgs::WaypointList waypoints_pipline, double yaw, bool specified_yaw);
void simple_goal_planner(double target_x, double target_y, double target_z, double yaw, bool specified_yaw);
void continuously_goal_planner(double target_x, double target_y, double target_z);